package com.yc.droid;

/**
 * Created by Administrator on 2016/4/27 0027.
 */
public class DataFrame {
    static int bad_line = 0;
    public static int front_line_num = 0;
    static int[] one_frame;
    static final float q30 = 1.07374182E9f;
    static int total_frame;
    static int total_line;

    static {
        DataFrame.one_frame = new int[30];
        DataFrame.front_line_num = 0;
    }

    public static int come_one_line(final char[] array) {
        ++DataFrame.total_line;
        if (array[0] != 238 || array[1] != 34) {
            ++DataFrame.bad_line;
            return 0;
        }
        if (array[2] % 2 == 0) {
            for (int i = 0; i < 15; ++i) {
                DataFrame.one_frame[i] = array[i + 3];
            }
            DataFrame.front_line_num = array[2];
            return 1;
        }
        if (DataFrame.front_line_num == -1 + array[2]) {
            for (int j = 0; j < 15; ++j) {
                DataFrame.one_frame[j + 15] = array[j + 3];
            }
            ++DataFrame.total_frame;
            return 2;
        }
        return 0;
    }

    public static String getDataString() {
        return DMP.initDMP(DataFrame.one_frame);
    }

    public static void init() {
        DataFrame.total_line = 0;
        DataFrame.bad_line = 0;
        DataFrame.total_frame = 0;
    }

    private static class DMP
    {
        static short[] accel;
        static short[] gyro;
        public static ACCEL_CONFIG mACCEL_CONFIG;
        public static GYRO_CONFIG mGYRO_CONFIG;
        static double pitch;
        static float q0;
        static float q1;
        static float q2;
        static float q3;
        static int[] quat;
        static double roll;
        static int times;
        static double yaw;

        static {
            DMP.quat = new int[4];
            DMP.accel = new short[3];
            DMP.gyro = new short[3];
            DMP.q0 = 1.0f;
            DMP.q1 = 0.0f;
            DMP.q2 = 0.0f;
            DMP.q3 = 0.0f;
            DMP.mACCEL_CONFIG = ACCEL_CONFIG.ACCEL_CONFIG0;
            DMP.mGYRO_CONFIG = GYRO_CONFIG.GYRO_CONFIG3;
        }

        public static double getReal(int n, final boolean b) {
            if ((0x8000 & n) != 0x0) {
                n -= 65536;
            }
            if (b) {
                if (DMP.mACCEL_CONFIG == ACCEL_CONFIG.ACCEL_CONFIG0) {
                    return 4.0 * n / 65536.0;
                }
                if (DMP.mACCEL_CONFIG == ACCEL_CONFIG.ACCEL_CONFIG1) {
                    return 8.0 * n / 65536.0;
                }
                if (DMP.mACCEL_CONFIG == ACCEL_CONFIG.ACCEL_CONFIG2) {
                    return 16.0 * n / 65536.0;
                }
                if (DMP.mACCEL_CONFIG == ACCEL_CONFIG.ACCEL_CONFIG3) {
                    return 32.0 * n / 65536.0;
                }
            }
            else {
                if (DMP.mGYRO_CONFIG == GYRO_CONFIG.GYRO_CONFIG0) {
                    return 500.0 * n / 65536.0;
                }
                if (DMP.mGYRO_CONFIG == GYRO_CONFIG.GYRO_CONFIG1) {
                    return 1000.0 * n / 65536.0;
                }
                if (DMP.mGYRO_CONFIG == GYRO_CONFIG.GYRO_CONFIG2) {
                    return 2000.0 * n / 65536.0;
                }
                if (DMP.mGYRO_CONFIG == GYRO_CONFIG.GYRO_CONFIG3) {
                    return 4000.0 * n / 65536.0;
                }
            }
            return 666666.0;
        }

        public static String initDMP(final int[] array) {
            DMP.times = (array[0] << 8) + array[1];
            DMP.quat[0] = (array[2] << 24 | array[3] << 16 | array[4] << 8 | array[5]);
            DMP.quat[1] = (array[6] << 24 | array[7] << 16 | array[8] << 8 | array[9]);
            DMP.quat[2] = (array[10] << 24 | array[11] << 16 | array[12] << 8 | array[13]);
            DMP.quat[3] = (array[14] << 24 | array[15] << 16 | array[16] << 8 | array[17]);
            DMP.accel[0] = (short)(array[18] << 8 | array[19]);
            DMP.accel[1] = (short)(array[20] << 8 | array[21]);
            DMP.accel[2] = (short)(array[22] << 8 | array[23]);
            DMP.gyro[0] = (short)(array[24] << 8 | array[25]);
            DMP.gyro[1] = (short)(array[26] << 8 | array[27]);
            DMP.gyro[2] = (short)(array[28] << 8 | array[29]);
            DMP.q0 = DMP.quat[0] / 1.07374182E9f;
            DMP.q1 = DMP.quat[1] / 1.07374182E9f;
            DMP.q2 = DMP.quat[2] / 1.07374182E9f;
            DMP.q3 = DMP.quat[3] / 1.07374182E9f;
            DMP.pitch = 57.3 * Math.asin(-2.0f * DMP.q1 * DMP.q3 + 2.0f * DMP.q0 * DMP.q2);
            DMP.roll = 57.3 * Math.atan2(2.0f * DMP.q2 * DMP.q3 + 2.0f * DMP.q0 * DMP.q1, 1.0f + (-2.0f * DMP.q1 * DMP.q1 - 2.0f * DMP.q2 * DMP.q2));
            DMP.yaw = 57.3 * Math.atan2(2.0f * (DMP.q1 * DMP.q2 + DMP.q0 * DMP.q3), DMP.q0 * DMP.q0 + DMP.q1 * DMP.q1 - DMP.q2 * DMP.q2 - DMP.q3 * DMP.q3);
            return String.format("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f", DMP.times, DMP.quat[0], DMP.quat[1], DMP.quat[2], DMP.quat[3], DMP.accel[0], DMP.accel[1], DMP.accel[2], DMP.gyro[0], DMP.gyro[1], DMP.gyro[2], getReal(DMP.accel[0], true), getReal(DMP.accel[1], true), getReal(DMP.accel[2], true), getReal(DMP.gyro[0], false), getReal(DMP.gyro[1], false), getReal(DMP.gyro[2], false), DMP.pitch, DMP.roll, DMP.yaw);
        }

        public enum ACCEL_CONFIG
        {
            /**
             *
             */
            ACCEL_CONFIG0,
            ACCEL_CONFIG1,
            ACCEL_CONFIG2,
            ACCEL_CONFIG3
        }

        public enum GYRO_CONFIG
        {
            GYRO_CONFIG0,
            GYRO_CONFIG1,
            GYRO_CONFIG2,
            GYRO_CONFIG3
        }
    }
}
